#include "MOTOR_receive.h"
// #include "cmsis_os.h"
#include "main.h"
#include "arm_task.h"
// #include "bsp_rng.h"
// #include "detect_task.h"

extern FDCAN_HandleTypeDef hfdcan1;//目前只使用了fdcan1以及只使能了fdcan1中断
extern FDCAN_HandleTypeDef hfdcan2;
extern FDCAN_HandleTypeDef hfdcan3;
extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart1;
//motor data read
//读取RM电机数据包括底盘电机与M2006电机
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }

/*

 */
//声明电机变量
static motor_measure_t  motor_motor5, motor_motor6;
static DMMotor_measure_t DMMotor_joint1,DMMotor_joint4;
static MOTOR_recv UNMotor_joint2,UNMotor_joint3;
MOTOR_recv MOTOR_recv_date;//数据接收结构体

static FDCAN_TxHeaderTypeDef  arm_tx_message;//发送机械臂消息类型定义
static uint8_t              arm_fdcan_send_data[8]; //fdcan发送消息数组
static int16_t motor5_ecd_count = 0;
static int16_t motor6_ecd_count = 0;
/**
  * @brief          hal库FDCAN回调函数,接收电机数据
  * @param[in]      hfdcan:CAN句柄指针
  * @retval         参考版本需要验证
  */
 
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
   FDCAN_RxHeaderTypeDef RxHeader;
   uint8_t RxData[8]; 

  if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
  {
    /* Retrieve Rx messages from RX FIFO0 */
    if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK)
    {
      Error_Handler();
    }
    switch (RxHeader.Identifier)
    {   
 /*------------------------机械臂FDCAN通信电机---------------*/
        case FDCAN_2006_J6_ID:
        {
        //处理电机数据宏函数
        get_motor_measure(&motor_motor6, RxData);
		    if(motor_motor6.speed_rpm > 0)                          //如果转子速度为正
          {
          if (motor_motor6.ecd - motor_motor6.last_ecd < -HALF_ECD_RANGE)
          {
          motor6_ecd_count++;
          }
          }
  		  else if(motor_motor6.speed_rpm < 0)
        {
          if (motor_motor6.ecd - motor_motor6.last_ecd > HALF_ECD_RANGE)
          {
          motor6_ecd_count--;
          }
        }
        motor_motor6.ecd_count=motor6_ecd_count;
        break;
        }
        case FDCAN_2006_J5_ID:
        {
        //处理电机数据宏函数
        get_motor_measure(&motor_motor5, RxData);
		    if(motor_motor5.speed_rpm > 0)                          //如果转子速度为正
          {
          if (motor_motor5.ecd - motor_motor5.last_ecd < -HALF_ECD_RANGE)
          {
          motor5_ecd_count++;
          }
          }
  		  else if(motor_motor5.speed_rpm < 0)
        {
          if (motor_motor5.ecd - motor_motor5.last_ecd > HALF_ECD_RANGE)
          {
          motor5_ecd_count--;
          }
        }
        motor_motor5.ecd_count=motor5_ecd_count;
        break;
        }        
        case FDCAN_DM_J1_ID:
        {
        get_DMMotor_measure(&DMMotor_joint1, RxData);
        DMMotor_joint1.pos = uint_to_float(DMMotor_joint1.pos_int, P_MIN, P_MAX, 16);
        DMMotor_joint1.vel = uint_to_float(DMMotor_joint1.vel_int, V_MIN, V_MAX, 12);
        DMMotor_joint1.tor = uint_to_float(DMMotor_joint1.tor_int, T_MIN, T_MAX, 12);
        break;
        }
        case FDCAN_DM_J4_ID:
        {
        get_DMMotor_measure(&DMMotor_joint4, RxData);
        DMMotor_joint4.pos = uint_to_float(DMMotor_joint4.pos_int, P_MIN, P_MAX, 16);
        DMMotor_joint4.vel = uint_to_float(DMMotor_joint4.vel_int, V_MIN, V_MAX, 12);
        DMMotor_joint4.tor = uint_to_float(DMMotor_joint4.tor_int, T_MIN, T_MAX, 12);
        break;
        }     
        default:
        {
            break;
        }
    }
  }
}

//串口中断处理函数，树宇电机的数据接收
void USART2_IRQHandler(void)
{
    uint16_t rxlen = 0;
  /* USER CODE BEGIN USART2_IRQn 0 */

     if((__HAL_UART_GET_FLAG(&huart2,UART_FLAG_RXNE)!=RESET))
   {

     HAL_UARTEx_ReceiveToIdle(&huart2, (uint8_t *)&MOTOR_recv_date, sizeof(MOTOR_recv_date.motor_recv_data), &rxlen, 0xFFFF);
    if(rxlen == sizeof(MOTOR_recv_date.motor_recv_data))
    {
      uint8_t *rp = (uint8_t *)&MOTOR_recv_date.motor_recv_data;//取电机接收数据结构体地址 MotorData_t motor_recv_data;
      if(rp[0] == 0xFD && rp[1] == 0xEE)
      {
        extract_data(&MOTOR_recv_date,&UNMotor_joint2,&UNMotor_joint3);
      }
    }
   }
    HAL_UART_IRQHandler(&huart2);
    __HAL_UART_ENABLE_IT(&huart2,UART_IT_ERR);
    __HAL_UART_ENABLE_IT(&huart2,UART_IT_RXNE);
}


/*-----------------------------------机械臂电机的控制发送函数------------------------------------*/
/**
  * @brief          发送RM电机控制电流(0x205,0x206,0x207,0x208)，发送机械臂J5，J6的M2006电机控制命令，其中rev为保留字节
  *                 M2006为RM电机，一个can数据帧可以发送四个RM电机的控制电流,保留位后面看一下吸盘电机是否为RM电机，若是可以利用起来
  * @param[in]      motor5: (0x207) 2006电机控制电流, 范围 [-10000,10000]
  * @param[in]      motor6: (0x207) 2006电机控制电流, 范围 [-10000,10000]
  * @param[in]      rev: (0x208) 保留，电机控制电流
  * @param[in]      rev: (0x208) 保留，电机控制电流
  * @retval         none
  */

void FDCAN_M2006_cmd_arm(int16_t motor5, int16_t motor6,int16_t rev1,int16_t rev2)
{
    arm_tx_message.Identifier=FDCAN_ARM_ALL_ID;              //32位ID
    arm_tx_message.IdType=FDCAN_STANDARD_ID;                  //标准ID
    arm_tx_message.TxFrameType=FDCAN_DATA_FRAME;              //数据帧
    arm_tx_message.DataLength=FDCAN_DLC_BYTES_8;              //数据长度8bit
    arm_tx_message.ErrorStateIndicator=FDCAN_ESI_ACTIVE;            
    arm_tx_message.BitRateSwitch=FDCAN_BRS_OFF;               //关闭速率切换
    arm_tx_message.FDFormat=FDCAN_CLASSIC_CAN;                //传统的CAN模式
    arm_tx_message.TxEventFifoControl=FDCAN_NO_TX_EVENTS;     //无发送事件
    arm_tx_message.MessageMarker=0; 
    arm_fdcan_send_data[0] = (motor5 >> 8);
    arm_fdcan_send_data[1] = motor5;
    arm_fdcan_send_data[2] = (motor6 >> 8);
    arm_fdcan_send_data[3] = motor6;
    arm_fdcan_send_data[4] = (rev1 >> 8);
    arm_fdcan_send_data[5] = rev1;
    arm_fdcan_send_data[6] = (rev2 >> 8);
    arm_fdcan_send_data[7] = rev2;
    HAL_FDCAN_AddMessageToTxFifoQ(&ARM_FDCAN,&arm_tx_message,arm_fdcan_send_data);
}
/*********** 
达妙电机DM4310的发送与接收在的源代码在application\MOTOR_receive\MOTOR_box\DM4310文件夹中
宇树电机M8010的发送与接收在的源代码在application\MOTOR_receive\MOTOR_box\GO-M8010-6文件夹中
************/


/*---------------------------通过指针方式获取原始数据--------------------------------------*/
//返回joint1电机变量地址，通过指针方式获取原始数据
const DMMotor_measure_t *get_DM_Jiont1_Measure_Point(void)
{
    return &DMMotor_joint1;
}
//返回joint4电机变量地址，通过指针方式获取原始数据
const DMMotor_measure_t *get_DM_Joint4_Measure_Point(void)
{
    return &DMMotor_joint4;
}
//返回motor5电机变量地址，通过指针方式获取原始数据
const motor_measure_t *get_RM_motor5_Motor_Measure_Point(void)
{
    return &motor_motor5;
}
//返回motor6电机变量地址，通过指针方式获取原始数据
const motor_measure_t *get_RM_motor6_Motor_Measure_Point(void)
{
    return &motor_motor6;
}
// //返回joint2电机变量地址，通过指针方式获取树宇电机原始数据
extern const MOTOR_recv *get_UN_Joint2_Motor_Measure_Point(void)
{
    return &UNMotor_joint2;
}
// //返回joint3电机变量地址，通过指针方式获取树宇电机原始数据
extern const MOTOR_recv *get_UN_Joint3_Motor_Measure_Point(void)
{
    return &UNMotor_joint3;
}

